# -*- coding: utf-8 -*-
import rospy
import collections
import glob
import math
import os
import sys
import time
import weakref
import pyproj
import carla
from v2x_msgs.msg import RSI, RTEData, RTSData

# Connect to client
client = carla.Client('192.168.3.157', 2000)
client.set_timeout(2.0)

world = client.get_world()
current_map = world.get_map()
actors = world.get_actors()

# 将xy坐标转换为gps坐标
geod = pyproj.Geod(ellps = "WGS84")     
def xy2gps(x,y,elevation):
    dis = math.sqrt(math.pow(x,2)+math.pow(y,2))
    azm = math.degrees(math.atan2(-y,x))
    #设置（125.1550768,43.8334643）为初始点坐标
    lon, lat, re_back_azm = geod.fwd(125.1550768,43.8334643,azm,dis)
    return lon*10000000, lat*10000000, elevation

def get_gps(actor):
    t = actor.get_transform()
    return xy2gps(t.location.x,t.location.y,t.location.z)

rte_list = []

def collision_listener():
    collision = None
    for actor in actors:
        if 'sensor.other.collision' == actor.type_id:
            collision = actor
    collision.listen(lambda event: _on_collision(event))

def get_actor_display_name(actor, truncate=250):
    name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:])
    return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name

def _on_collision(event):
    actor_type = get_actor_display_name(event.other_actor)
    impulse = event.normal_impulse
    intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2)
    if(intensity > 100):
        rteData = RTEData()
        rteData.eventType = 1
        rteData.eventSource = 1
        rteData.present = 7
        rteData.lon, rteData.lat, ignored= get_gps(event.actor)
        rte_list.append(rteData)

def talker():
    rts = []
    for actor in actors:
        if 'traffic.speed_limit.30' == actor.type_id:
            rtsData = RTSData()
            rtsData.rtsid = actor.id
            rtsData.signtype = 1
            rtsData.present = 7
            rtsData.lon, rtsData.lat, ignored= get_gps(actor)
            rts.append(rtsData)
        if 'traffic.speed_limit.60' == actor.type_id:
            rtsData = RTSData()
            rtsData.rtsid = actor.id
            rtsData.signtype = 2
            rtsData.present = 7
            rtsData.lon, rtsData.lat, ignored= get_gps(actor)
            rts.append(rtsData)
        if 'traffic.speed_limit.90' == actor.type_id:
            rtsData = RTSData()
            rtsData.rtsid = actor.id
            rtsData.signtype = 3
            rtsData.present = 7
            rtsData.lon, rtsData.lat, ignored= get_gps(actor)
            rts.append(rtsData)
    pub = rospy.Publisher('rsi', RSI, queue_size=10)
    rospy.init_node('rsi', anonymous=True)
    rate = rospy.Rate(1) # 1hz
    while not rospy.is_shutdown():
        rsi = RSI()
        rsi.msgcnt = 1
        rsi.id = "1"
        rsi.rsulon, rsi.rsulat, eve= xy2gps(95,205,0) 
        rsi.rts_list = rts
        global rte_list
        rsi.rte_list = rte_list
        rte_list = []

        pub.publish(rsi)

        rospy.loginfo("published!")
        rate.sleep()
        
talker()